#include "threadmanager.h"
#include "uinterface.h"
#include <cstdlib>
#include <cstring>
#include <QDebug>
#include <cmath>


#define mlxWIDTH 32
#define mlxHEIGHT 24
#define TEMMAX 85
#define TEMMIN 0

void DataReader::readPortData() {

    static int cnt=0;
    ++cnt;
    while (true) {
#if 0
        /* simulate */
//        FILE * f = fopen("data.txt","r");
//        if (!f) {
//            qDebug()<<"file open failed";
//            continue;
//        }
        for (int i=0;i<1544;i++) {
            dataBuffer[i] = rand();
        }
        while(parent->processor->isRunning){
            //wait
        }
        parent->processor->isRunning=true;
        emit reader2processor_readDone();
        //fclose(f);
#endif

        /* read data */
        assert(parent->port!=HANDLE(-1));
        uInterface::ReadXBytesFromPort(parent->port, dataBuffer, 1544);
        if (!(dataBuffer[0]==0x5A&&dataBuffer[1]==0x5A
              &&dataBuffer[2]==0x02&&dataBuffer[3]==0x06)) {
            for (int i=0;i<1544;i++)
                qDebug()<<i<<dataBuffer[i];
            assert(dataBuffer[0]==0x5A&&dataBuffer[1]==0x5A
                    &&dataBuffer[2]==0x02&&dataBuffer[3]==0x06);
//            qDebug()<<"Read Error in DataReader::readData."<<endl;
            return;
        }
        while(parent->processor->isRunning){
            //wait
        }
        parent->processor->isRunning=true;
        emit reader2processor_readDone();
    }

}


static float Fmax(float* arr, int num);
static float Fmin(float* arr, int num);
static int RejectWrongPixel(float* mlx90640To);
static float* AroundPixel(float *mlx90640To, int *size, int i, int j);
static bool isWrongPixel(float pixel, float* around, int size);
static QRgb toRGB(float temper, float max, float min);


void DataProcessor::processData() {
        static  bool showInfo = false;
    static int cnt = 0;
    ++cnt;
    for (int i=0;i<1544;i++)
        dataBuffer[i]=this->parent->reader->dataBuffer[i];

    /* data process */
    uint16_t TA;
    for (int i = 0; i < 768; i++) {
        dataFrame[i] = (uint16_t(dataBuffer[i*2+3])<<8) + dataBuffer[i*2+2];
    }
    TA =  (uint16_t(dataBuffer[1541])<<8) + uint16_t(dataBuffer[1540]);
    for (int i = 0; i < 768; i++) {
        mlx90640To[i] = dataFrame[i / 32 * 32 + 31 - i % 32] / 100.0f;
    }

#if 0
    FILE* f =fopen("data.txt","r");
    if (!f) {
        qDebug()<<"file open err";
        while(1);
    }

    //第i行第j列
    for (int i=0;i<24;i++)
        for (int j=0;j<32;j++){
            fscanf(f,"%f",&mlx90640To[i*32+j]);
        }
    fclose(f);

#endif
    if (RejectWrongPixel(mlx90640To) > 5) {
        isRunning = false;
        return;
    }

    float _max = Fmax(mlx90640To, mlxHEIGHT * mlxWIDTH);
    float _min = Fmin(mlx90640To, mlxHEIGHT * mlxWIDTH);

// before this block, mat is not available
    while(this->parent->isSettingData) { // wait for OpenGL
    }
    matAvailbale = false;
    for (int i = 0; i < mlxHEIGHT; i++) {//Copy and Calculate RGB
        for (int j = 0; j < mlxWIDTH; j++) {
            mat[i][j] = mlx90640To[i * mlxWIDTH + j];
            DWORD pixelRGB = toRGB(mat[i][j], _max, _min);
            matRGB[i][j][0] = pixelRGB & 0x000000FF;//blue
            matRGB[i][j][1] = (pixelRGB & 0x0000FF00) >> 8;//green
            matRGB[i][j][2] = (pixelRGB & 0x00FF0000) >> 16;//red
        }
    }
    matAvailbale = true;
// after this block, mat is available

    for (int i = 0, ii = 0; i < newHeight; i += resolution, ii++) {//sign in
        for (int j = 0, jj = 0; j < newWidth; j += resolution, jj++) {
            for (int u = 0; u < resolution; u++) {
                for (int v = 0; v < resolution; v++) {
                    img[i + u][j + v] = mat[ii][jj];
                    for (int k = 0; k < 3; k++)
                        imgRGB[i + u][j + v][k] = matRGB[ii][jj][k];
                }
            }
        }
    }
    for (int i = 0; i < newHeight; i++) {//process Image data
        for (int j = 0; j < newWidth; j++) {
            DWORD avIr = 0, avIg = 0, avIb = 0;
            int pixCnt = 0;
            for (int u = i - resolution ; u < i + resolution ; u++) {
                if (u < 0 || u >= newHeight) continue;
                for (int v = j - resolution ; v < j + resolution ; v++) {
                    if (v < 0 || v >= newWidth) continue;
                    avIr += imgRGB[u][v][2];
                    avIg += imgRGB[u][v][1];
                    avIb += imgRGB[u][v][0];
                    pixCnt++;
                }
            }

            avIr /= pixCnt;;
            avIg /= pixCnt;
            avIb /= pixCnt;

            outImgRGB[i][j] = (0xFF000000|(avIr << 16) | (avIg << 8) | avIb);
        }
    }

    //

    for (int i=0;i<newHeight;i++) {
        for (int j=0;j<newWidth;j++) {
            /*
             * [i,j][i,j+1][i,j+2]
             * [i+1,j][i+1,j+1][i+1,j+2]
             * [i+2,j][i+2,j+1][i+2,j+2]
             */
            for (int u=6*i;u<6*i+6;u++)
                for (int v=6*j;v<6*j+6;v++) {
                    pixels[u*image_param::imageWidth+v] = outImgRGB[i][j];
                }
        }
    }
    if (showInfo) {
        double __tempmax=-1;
        qDebug()<<this->parent->emmity;
        for (int i=0;i<24;i++) {
            for (int j=0;j<32;j++) {
                if (mat[i][j]>__tempmax) __tempmax=mat[i][j];
                qDebug()<<i<<j<<":"<<mat[i][j];
            }
        }
        qDebug()<<"max temperature:"<<__tempmax;
        showInfo = false;
    }
    emit processor2manager_draw();
    if (!(this->parent->writeDone)){
        int e=this->parent->emmity*100;
        BYTE by = BYTE(e);
//        qDebug()<<by;
        //0xA5 +0x45 +0xXX+sum
        int sum=0;
        BYTE emit_buffer[4] = {0xA5,0x45};
        emit_buffer[2]=by;
        sum=by+0xA5+0x45;
        emit_buffer[3]=sum&0xff;
        DWORD wsize;
        int success = WriteFile(this->parent->port,emit_buffer,4,&wsize,NULL);
        this->parent->writeDone = true;
//        qDebug()<<wsize;
        assert(success!=0&&wsize==4);
        showInfo = true;

    }
    isRunning = false;
}














/* =================================== */
static float Fmax(float* arr, int num) {
    float _max = 0;
    for (int i = 0; i < num; i++){
        if (arr[i] > _max) _max = arr[i];
    }
    return _max;
}
static float Fmin(float* arr, int num) {
    float _min = 80;
    for (int i = 0; i < num; i++){
        if (arr[i] < _min) _min = arr[i];
    }
    return _min;
}

static int RejectWrongPixel(float* mlx90640To) {
    for (int i = 0; i < mlxHEIGHT; i++){
        for (int j = 0; j < mlxWIDTH; j++){
            if (_isnan(mlx90640To[i * mlxWIDTH + j])) {
                int size;
                float *around = AroundPixel(mlx90640To, &size, i, j);
                for (int k = 0; k < size; k++){
                    if (_isnan(around[k]))return 6;
                }
                mlx90640To[i * mlxWIDTH + j] = 0;
                for (int k = 0; k < size; k++) {
                    mlx90640To[i * mlxWIDTH + j] += around[k] / size;
                }
            }
        }
    }
    float *around;
    int size;
    int WrongPixel = 0;
    for (int i = 0; i < mlxHEIGHT; i++)
    {
        for (int j = 0; j < mlxWIDTH; j++)
        {
            around = AroundPixel(mlx90640To, &size, i, j);
            if (isWrongPixel(mlx90640To[i * mlxWIDTH+ j], around, size))
            {
                WrongPixel++;
                mlx90640To[i * mlxWIDTH + j] = 0;
                for (int k = 0; k < size; k++) mlx90640To[i * mlxWIDTH + j] += around[k] / size;
            }
        }
    }
    return WrongPixel;
}
static float* AroundPixel(float *mlx90640To, int *size, int i, int j) {
    if (i > 0 && i < mlxHEIGHT-1 && j > 0 && j < mlxWIDTH-1){
        *size = 8;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j - 1];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[2] = mlx90640To[(i - 1) * mlxWIDTH + j + 1];
        around[3] = mlx90640To[i * mlxWIDTH + j - 1];
        around[4] = mlx90640To[i * mlxWIDTH + j + 1];
        around[5] = mlx90640To[(i + 1) * mlxWIDTH + j - 1];
        around[6] = mlx90640To[(i + 1) * mlxWIDTH + j];
        around[7] = mlx90640To[(i + 1) * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i == 0 && j > 0 && j < mlxWIDTH-1) {
        *size = 5;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[i * mlxWIDTH + j - 1];
        around[1] = mlx90640To[i * mlxWIDTH + j + 1];
        around[2] = mlx90640To[(i + 1) * mlxWIDTH + j - 1];
        around[3] = mlx90640To[(i + 1) * mlxWIDTH + j];
        around[4] = mlx90640To[(i + 1) * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i == mlxHEIGHT-1 && j > 0 && j < mlxWIDTH-1) {
        *size = 5;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j - 1];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[2] = mlx90640To[(i - 1) * mlxWIDTH + j + 1];
        around[3] = mlx90640To[i * mlxWIDTH + j - 1];
        around[4] = mlx90640To[i * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i > 0 && i < mlxHEIGHT-1 && j == 0) {
        *size = 5;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j + 1];
        around[2] = mlx90640To[i * mlxWIDTH + j + 1];
        around[3] = mlx90640To[(i + 1) * mlxWIDTH + j];
        around[4] = mlx90640To[(i + 1) * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i > 0 && i < mlxHEIGHT-1 && j == mlxWIDTH-1) {
        *size = 5;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j - 1];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[2] = mlx90640To[i * mlxWIDTH + j - 1];
        around[3] = mlx90640To[(i + 1) * mlxWIDTH + j - 1];
        around[4] = mlx90640To[(i + 1) * mlxWIDTH + j];
        free(around);
        return around;
    }
    if (i == 0 && j == 0) {
        *size = 3;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[i * mlxWIDTH + j + 1];
        around[1] = mlx90640To[(i + 1) * mlxWIDTH + j];
        around[2] = mlx90640To[(i + 1) * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i == mlxHEIGHT-1 && j == 0) {
        *size = 3;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j + 1];
        around[2] = mlx90640To[i * mlxWIDTH + j + 1];
        free(around);
        return around;
    }
    if (i == 0 && j == mlxWIDTH-1) {
        *size = 3;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[i * mlxWIDTH + j - 1];
        around[1] = mlx90640To[(i + 1) * mlxWIDTH + j - 1];
        around[2] = mlx90640To[(i + 1) * mlxWIDTH + j];
        free(around);
        return around;
    }
    if (i == mlxHEIGHT-1 && j == mlxWIDTH-1) {
        *size = 3;
        float *around = (float*)malloc(sizeof(float)*(*size));
        around[0] = mlx90640To[(i - 1) * mlxWIDTH + j - 1];
        around[1] = mlx90640To[(i - 1) * mlxWIDTH + j];
        around[2] = mlx90640To[i * mlxWIDTH + j - 1];
        free(around);
        return around;
    }
}
static QRgb toRGB(float temper, float max, float min) {
    float a = temper - min;
    float b = temper - (2 * min / 3 + max / 3);
    float c = temper - (min / 3 + 2 * max / 3);
    float d = max - min;
    float e = (min + max) / 2;
    DWORD myred, mygreen, myblue;

#if 0
    if (temper < 20) {
        myred = 0;
        mygreen = temper / 20 * 255;
        myblue = 255;
    }
    if (temper < 60 && temper >= 20) {
        myred = fcmax(0, 255 * (temper - 40) / 20);
        mygreen = 255;
        myblue = fcmax(0, 255 * (1 - (temper - 20) / 20));
    }
    else {
        myred = 255;
        mygreen = fcmax(0, 255 - 255 * (temper - 60) / 20);
        myblue = 0;
    }
#endif

    if (a >= 0 && b <= 0) {
        myred = 0;
        mygreen = 100 + (3 * a / d * 155);
        myblue = 255;
    }
    else if (b > 0 && c <= 0) {
        myred = 0;
        mygreen = 255;
        myblue = 255 * (1 - (3 * b / d));
    }
    else if (c > 0 && temper <= max) {
        myred = 255;
        mygreen = 255 - 255 * 3 * c / d;
        myblue = 0;
    }
    return QRgb((0xFF<<24)|(myred << 16) | (mygreen << 8) | myblue);
}
static bool isWrongPixel(float pixel, float* around, int size) {\
    if (pixel > TEMMAX || pixel < TEMMIN)return 1;
    bool tag = 1;
    for (int i = 0; i < size; i++) {
        if (pixel - around[i] < 13 && pixel - around[i] > -13)tag = 0;
    }
    return tag;
}
